/**
 * @brief 			Controller.cpp Member Function Implementations
 */

#include "FRules.h"
#include "Types.h"

#include <iostream>
#include <iomanip>
#include <cmath>

const unsigned int THRESHOLD_FARAWAY = 200; // far away
const unsigned int THRESHOLD_MEDIUM = 600;
const unsigned int THRESHOLD_NEAR = 900; // near

const double WEIGHT_COEFF = 50;
const double SPEED = 0.8;
const double START_VALUE = 100.0;

FRules::FRules()
{
  for(int i = 0; i < NUM_SENS; ++i)
    for(int j = 0; j < NUM_ACTION; ++j)
      weight[i][j] = START_VALUE;
}

FRules::~FRules()
{
}

void FRules::input(const Sensors& values)
{
  double actor_sum[NUM_ACTION];

  for(int i = 0; i < NUM_SENS; ++i)
  {
    if(values[i] <= THRESHOLD_FARAWAY)
    {
      std::cout << "inside far far away\n";
      Action s = queue.front();
      switch(s)
      {
        case ALEFT:
        {
          if(weight[i][ALEFT] <500)
          {
          weight[i][ALEFT] += WEIGHT_COEFF/100;
          }
          break;
        }
        case ARIGHT:
        {
          if(weight[i][ARIGHT] <500)
          {
            weight[i][ARIGHT] += WEIGHT_COEFF/100;
          }
          break;
        }
        case AFORWARD:
        {
          if(weight[i][AFORWARD] <500)
          {
            weight[i][AFORWARD] += WEIGHT_COEFF/100;
          }
          break;
        }
        case ABACKWARD:
        {
          if(weight[i][ABACKWARD] <500)
          {
            weight[i][ABACKWARD] += WEIGHT_COEFF/100;
          }
          break;
        }
        case NUM_ACTION: break;
      }
      continue;
    }
    else if(values[i] > THRESHOLD_FARAWAY && values[i] <= THRESHOLD_MEDIUM)
    {
      std::cout << "inside medium\n";
      for(int j = 0; j < NUM_ACTION; ++j)
        actor_sum[j] += weight[i][j];
    }
    else if(values[i] > THRESHOLD_MEDIUM && values[i] <= THRESHOLD_NEAR)
    {
      std::cout << "inside near away\n";
      for(int j = 0; j < NUM_ACTION; ++j)
        actor_sum[j] += weight[i][j]*2;
    }
    else
    {
      std::cout << "inside extreme near\n";
      Action t = queue.front();
      switch(t)
      {
        case ALEFT:
        {
          weight[i][ALEFT] -= WEIGHT_COEFF;
          actor_sum[ARIGHT] += WEIGHT_COEFF;
          break;
        }
        case ARIGHT:
        {
          weight[i][ARIGHT] -= WEIGHT_COEFF;
          actor_sum[ALEFT] += WEIGHT_COEFF;
          break;
        }
        case AFORWARD:
        {
          weight[i][AFORWARD] -= WEIGHT_COEFF;
          actor_sum[ABACKWARD] += WEIGHT_COEFF;
          break;
        }
        case ABACKWARD:
        {
          weight[i][ABACKWARD] -= WEIGHT_COEFF;
          actor_sum[AFORWARD] += WEIGHT_COEFF;
          break;
        }
        case NUM_ACTION: break;
      }
    }
  }

  if(queue.front() == ALEFT)
    actor_sum[ARIGHT] -= 50;
  else if(queue.front() == ARIGHT)
    actor_sum[ALEFT] -= 50;
  else if(queue.front() == AFORWARD)
    actor_sum[ABACKWARD] -= 50;
  else if(queue.front() == ABACKWARD)
    actor_sum[AFORWARD] -= 50;

  Action tmp = ALEFT;
  for(int j = 1; j < NUM_ACTION; ++j)
  {
    if(actor_sum[j] > actor_sum[tmp])
      tmp = static_cast<Action>(j);
  }

  std::cout << "gewichte:\n";
  for(int j = 0; j < NUM_ACTION; ++j)
  {
    std::cout << std::setw(9) <<  printAction(static_cast<Action>(j));
    for(int i = 0; i < NUM_SENS; ++i)
      std::cout << std::setw(7) << weight[i][j];

    std::cout << "\n";
  }
  std::cout << "\n";

  std::cout << "biggest: " << printAction(tmp) << std::endl;
  if(queue.size() > 0)
    queue.pop();

  queue.push(tmp);

//  double left = fabs(actor_sum[AFORWARD])/fabs(actor_sum[ABACKWARD]) *
//                actor_sum[AFORWARD] * actor_sum[ABACKWARD] * actor_sum[ARIGHT];

  double left = ( actor_sum[AFORWARD] - actor_sum[ABACKWARD] ) / (actor_sum[ALEFT] - actor_sum[ARIGHT]);
  double right = ( actor_sum[AFORWARD] - actor_sum[ABACKWARD] ) / (actor_sum[ARIGHT] - actor_sum[ALEFT]);
//  double right = fabs(actor_sum[AFORWARD])/fabs(actor_sum[ABACKWARD]) *
//                actor_sum[AFORWARD] * actor_sum[ABACKWARD] * actor_sum[ALEFT];


  std::cout << "left: " << left << " right: " << right << "\n";
  motor.move(left, right);

//  switch(tmp)
//  {
//    case ALEFT: motor.move(SPEED, -SPEED); break;
//    case ARIGHT: motor.move(-SPEED, SPEED); break;
//    case AFORWARD: motor.move(SPEED, SPEED); break;
//    case ABACKWARD: motor.move(-SPEED, -SPEED); break;
//    case NUM_ACTION: break;
//  }



//  std::cout << "---\n";
//  for(int j = 0; j < NUM_ACTION; ++j)
//    std::cout << printAction(static_cast<Action>(j)) << " sum: " << actor_sum[j] << ", ";

  std::cout << "\n";
}

Motor& FRules::output()
{
  motor.convertToMaxSpeed();
  return motor;
}
